#include "./box_motors.h"
#include "../../device/gpio.h"
#include "./sensor.h"
#include "../../utils/progress.h"
#include "../analog.h"

namespace driver {

using namespace device::gpio;
using namespace utils;

using DIR1 = PC<8>;
using DIR2 = PC<9>;
using UPDOWN = PC<10>;
using DUMP = PC<11>;

using PDE2 = PC<12>;
using PDE1 = PC<13>; // 压紧电磁铁 

BoxMotors::BoxMotors() {
    RCC->APB2ENR |= RCC_APB2ENR_IOPCEN;

    config_pp_out<DIR1, DIR2, UPDOWN, DUMP, PDE1, PDE2>();
}

void BoxMotors::pde_lock() {
    PDE1::clr();
    PDE2::set();
}

void BoxMotors::pde_unlock() {
    PDE1::set();
    PDE2::clr();
    osDelay(500);
    PDE1::clr();
    PDE2::clr();
}

Error BoxMotors::dump_reset(uint32_t duration) {
    Error err;
    Progress prog;
    uint32_t lock_count = 0;
    if (sensor->is_dump_top()) {
        goto finish;
    }
    set_dir(Dir::RESET);
    DUMP::set();
    for (;;) {
        osDelay(20);
        if (sensor->is_dump_top() || prog.is_after(duration)) {
            break;
        }
        if (prog.is_after(DUMP_TIMEOUT)) {
            err = ec::POWDER_DUMP_MOTOR_TIMEOUT;
            goto finish;
        }
       if (prog.is_after(500) && analog->is_dump_locked()) {
            lock_count ++;
            if (lock_count >= 10) {
                err = ec::POWDER_DUMP_MOTOR_LOCKED;
                goto finish;
            }
        } else {
            lock_count = 0;
        }
    }
finish:
    set_dir(Dir::BRAKE);
    DUMP::clr();
    return err;
}

Error BoxMotors::dump(uint32_t duration) {
    Error err;
    Progress prog;
    uint32_t lock_count = 0;
    if (sensor->is_dump_bottom()) {
        goto finish;
    }
    set_dir(Dir::DUMP);
    DUMP::set();
    for (;;) {
        osDelay(20);
        if (sensor->is_dump_bottom() || prog.is_after(duration)) {
            break;
        }
        if (prog.is_after(DUMP_TIMEOUT)) {
            err = ec::POWDER_UPDOWN_MOTOR_TIMEOUT;
            goto finish;
        }

       if (prog.is_after(500) && analog->is_dump_locked()) {
            lock_count ++;
            if (lock_count >= 10) {
                err = ec::POWDER_UPDOWN_MOTOR_LOCKED;
                goto finish;
            }
        } else {
            lock_count = 0;
        }
    }
finish:
    set_dir(Dir::BRAKE);
    DUMP::clr();
    return err;
}

Error BoxMotors::down(uint32_t duration) {
    Error err;
    Progress prog;
    uint32_t lock_count = 0;
    if (sensor->is_pick_bottom()) {
        goto finish;
    }
    set_dir(Dir::DOWN);
    UPDOWN::set();

    for (;;) {
        osDelay(20);
        if (sensor->is_pick_bottom() || prog.is_after(duration)) {
            break;
        }
        if (prog.is_after(UPDOWN_TIMEOUT)) {
            err = ec::POWDER_UPDOWN_MOTOR_TIMEOUT;
            goto finish;
        }
        if (prog.is_after(500) && analog->is_updown_locked()) {
            lock_count ++;
            if (lock_count >= 10) {
                err = ec::POWDER_UPDOWN_MOTOR_LOCKED;
                goto finish;
            }
        } else {
            lock_count = 0;
        }
    }

finish:
    set_dir(Dir::BRAKE);
    UPDOWN::clr();
    return err;
}

Error BoxMotors::up(uint32_t duration) {
    Error err;
    Progress prog;
    uint32_t lock_count = 0;
    if (sensor->is_pick_top()) {
        goto finish;
    }
    set_dir(Dir::UP);
    UPDOWN::set();

    for (;;) {
        osDelay(20);
        if (sensor->is_pick_top() || prog.is_after(duration)) {
            break;
        }
        if (prog.is_after(UPDOWN_TIMEOUT)) {
            err = ec::POWDER_UPDOWN_MOTOR_TIMEOUT;
            goto finish;
        }

        if (prog.is_after(500) && analog->is_updown_locked()) {
            lock_count ++;
            if (lock_count >= 10) {
                err = ec::POWDER_UPDOWN_MOTOR_LOCKED;
                goto finish;
            }
        } else {
            lock_count = 0;
        }

    }

finish:
    set_dir(Dir::BRAKE);
    UPDOWN::clr();
    return err;
}

void BoxMotors::set_dir(Dir dir) {
    switch (dir) {
        case Dir::UP:
            DIR1::set();
            DIR2::clr();
        break;

        case Dir::DOWN:
            DIR1::clr();
            DIR2::set();
        break;

        case Dir::BRAKE:
            DIR1::clr();
            DIR2::clr();
        break;
    }
}

}

